//
// Created by Zemon on 2024/11/2.
//

#include "lift.h"
#include "CAN_receive.h"
#include "remote_control.h"
#include "motor_pid.h"

extern motor_measure_t motor_chassis[5];
extern RC_ctrl_t rc_ctrl;
static float set_speed_temp;
float set_speed;
float set_angle;
float kps = 8.0f,kis = 0.0f,kds = 0.0f;
float kpa = 0.1f,kia = 0.0f,kda = 0.0f;
static int16_t lock_flag;

int16_t speed_delta;
static int16_t max_speed_change = 30000;
static pid_t pid_speed;
pid_t pid_angle;

void lift(void){
    if(rc_ctrl.rc.ch[1]!=0){
        lock_flag = 0;
    }
    else{
        lock_flag = 1;
    }
    PID_struct_init(&pid_speed,POSITION_PID,10000,0,kps,kis,kds);
    PID_struct_init(&pid_angle,POSITION_PID,15000,1500,kpa,kia,kda);

    if(lock_flag == 0){
        set_angle = motor_chassis[3].total_angle - rc_ctrl.rc.ch[1]*5;
        set_speed = -rc_ctrl.rc.ch[1];
        speed_delta = (int16_t)set_speed - motor_chassis[3].speed_rpm;
        if(speed_delta >max_speed_change)
            set_speed_temp = (float)(motor_chassis[3].speed_rpm+max_speed_change);
        else if(speed_delta < -max_speed_change)
            set_speed_temp = (float)(motor_chassis[3].speed_rpm-max_speed_change);
        else
            set_speed_temp = set_speed;
        pid_calc(&pid_speed,(float)motor_chassis[3].speed_rpm,set_speed_temp);

        CAN_cmd_lift((int16_t)(pid_speed.pos_out));
    }
    else if(lock_flag == 1){
        pid_calc(&pid_angle,(float)motor_chassis[3].total_angle,set_angle);
        set_speed = pid_angle.pos_out;
        speed_delta = (int16_t)set_speed - motor_chassis[3].speed_rpm;
        if(speed_delta>max_speed_change)
            set_speed_temp = (float)(motor_chassis[3].speed_rpm + max_speed_change);
        else if(speed_delta < -max_speed_change)
            set_speed_temp = (float)(motor_chassis[3].speed_rpm - max_speed_change);
        else
            set_speed_temp = set_speed;
        pid_calc(&pid_speed,(float)motor_chassis[3].speed_rpm,set_speed_temp);

        CAN_cmd_lift((int16_t)(pid_speed.pos_out));

    }
}